#include "matrix.h"

namespace MathLib
{
    //***Private Methods***

    /**
    *  Performs the element-wise generic comparison between \c this matrix and right hand one.
    *  \version 1.0.2a
    *  \date    8 February 2011
    *  \param [in] right_hand Right hand \c Matrix class.
    *  \param [in] operation A \c unsigned \c char, indicating the type of operation, with the following MACRO codes:\n
    *              - EQUAL_TO | ==\n
    *              - NOT_EQUAL_TO | !=\n
    *              - GREATER_THAN | >\n
    *              - GREATER_THAN_EQUAL_TO | >=\n
    *              - LOWER_THAN | <\n
    *              - LOWER_THAN_EQUAL_TO | <=\n
    *  \returns A \c bool \c Matrix class containing the result of the operation.
    *  \throws  SIZE_MISMATCH when matrices size does not agree.
    *  \throws  UNKNOWN_OPERATION when the specified operation code is not supported.
    */
    template <class DataType, typename LengthType, unsigned char dimensions>
           Matrix<bool,LengthType,dimensions> Matrix<DataType,LengthType,dimensions>::comparison
           ( const Matrix &right_hand, unsigned char operation )
    {
#define right_hand  ( ( Matrix& )right_hand )

        if( dimensions > 0 )
        {
#ifdef DEBUG
            checkSizeMismatch( right_hand );
#endif

            LengthType outSize[dimensions];

            for( i = 0; i < dimensions; i++ )
            {
                outSize[i] = size[i];
            }

            Matrix<bool,LengthType,dimensions> result( outSize );

            switch( operation )
            {
            case EQUAL_TO:
                for( i = 0; i < length; i++ )
                {
                    result(i) = ( data[i] == right_hand(i) );
                }
                break;
            case NOT_EQUAL_TO:
                for( i = 0; i < length; i++ )
                {
                    result(i) = ( data[i] != right_hand(i) );
                }
                break;
            case GREATER_THAN:
                for( i = 0; i < length; i++ )
                {
                    result(i) = ( data[i] > right_hand(i) );
                }
                break;
            case GREATER_THAN_EQUAL_TO:
                for( i = 0; i < length; i++ )
                {
                    result(i) = ( data[i] >= right_hand(i) );
                }
                break;
            case LOWER_THAN:
                for( i = 0; i < length; i++ )
                {
                    result(i) = ( data[i] < right_hand(i) );
                }
                break;
            case LOWER_THAN_EQUAL_TO:
                for( i = 0; i < length; i++ )
                {
                    result(i) = ( data[i] <= right_hand(i) );
                }
                break;
            default:
                THROW_EXCEPTION( className, UNKNOWN_OPERATION );
            }

            return result;
        }
        else
        {
            Matrix<bool,LengthType,dimensions> result;
            return result;
        }

#undef right_hand
    }


    /**
    *  Performs the element-wise generic comparison between \c this matrix and a right hand scalar value.
    *  \version 1.0.0a
    *  \date    8 February 2011
    *  \param [in] right_hand Right hand \c DataType value.
    *  \param [in] operation A \c unsigned \c char, indicating the type of operation, with the following MACRO codes:\n
    *              - EQUAL_TO | ==\n
    *              - NOT_EQUAL_TO | !=\n
    *              - GREATER_THAN | >\n
    *              - GREATER_THAN_EQUAL_TO | >=\n
    *              - LOWER_THAN | <\n
    *              - LOWER_THAN_EQUAL_TO | <=\n
    *  \returns A \c bool \c Matrix class containing the result of the operation.
    *  \throws  UNKNOWN_OPERATION when the specified operation code is not supported.
    */
    template <class DataType, typename LengthType, unsigned char dimensions>
           Matrix<bool,LengthType,dimensions> Matrix<DataType,LengthType,dimensions>::comparison
           ( const DataType &right_hand, unsigned char operation )
    {
        if( dimensions > 0 )
        {
            LengthType outSize[dimensions];

            for( i = 0; i < dimensions; i++ )
            {
                outSize[i] = size[i];
            }

            Matrix<bool,LengthType,dimensions> result( outSize );

            switch( operation )
            {
            case EQUAL_TO:
                for( i = 0; i < length; i++ )
                {
                    result(i) = ( data[i] == right_hand );
                }
                break;
            case NOT_EQUAL_TO:
                for( i = 0; i < length; i++ )
                {
                    result(i) = ( data[i] != right_hand );
                }
                break;
            case GREATER_THAN:
                for( i = 0; i < length; i++ )
                {
                    result(i) = ( data[i] > right_hand );
                }
                break;
            case GREATER_THAN_EQUAL_TO:
                for( i = 0; i < length; i++ )
                {
                    result(i) = ( data[i] >= right_hand );
                }
                break;
            case LOWER_THAN:
                for( i = 0; i < length; i++ )
                {
                    result(i) = ( data[i] < right_hand );
                }
                break;
            case LOWER_THAN_EQUAL_TO:
                for( i = 0; i < length; i++ )
                {
                    result(i) = ( data[i] <= right_hand );
                }
                break;
            default:
                THROW_EXCEPTION( className, UNKNOWN_OPERATION );
            }

            return result;
        }
        else
        {
            Matrix<bool,LengthType,dimensions> result;
            return result;
        }
    }


#ifdef DEBUG
    /**
    *  Checks that the length and the size of \c this matrix and the right_hand one are exactly the same.
    *  \version 1.0.0a
    *  \date    15 September 2010
    *  \param [in] right_hand Right hand \c Matrix class.
    *  \throws  SIZE_MISMATCH when matrices size does not agree.
    */
    template <class DataType, typename LengthType, unsigned char dimensions>
            void Matrix<DataType,LengthType,dimensions>::checkSizeMismatch( const Matrix &right_hand )
    {
        if( length != right_hand.length )
        {
            THROW_EXCEPTION( className, SIZE_MISMATCH );
        }

        for( i = 0; i < dimensions; i++ )
        {
            if( size[i] != right_hand.size[i] )
            {
                THROW_EXCEPTION( className, SIZE_MISMATCH );
            }
        }
    }
#endif


    //***Constructors and Destructor***

    /**
    *  Instantiates a \c Matrix class with the size of each dimension equal to 0.\n
    *  The empty matrix can be instantiated as follows:
    *  \code Matrix<double,unsigned int,0> A; \endcode
    *  \version 1.3.6a
    *  \date    28 January 2011
    *  \throw  MEMORY_ERROR when constructor cannot allocate enough memory for the \c Matrix class.
    *  \throw  DIMENSION_OVERFLOW when required dimensions exceed the maximum value allowed for the specified LengthType.
    */
    template <class DataType, typename LengthType, unsigned char dimensions>
            Matrix<DataType,LengthType,dimensions>::Matrix() : square(false), row(0), length(0), className( MATRIX_NAME )
    {
        switch( dimensions )
        {
        case 0:
            break;

        default:
            if( dimensions < 8 * sizeof( LengthType ) )
            {
                size = new (nothrow) LengthType[dimensions];

                if( size )
                {
                    memset(size, 0, dimensions * sizeof( LengthType ));
                }
                else
                {
                    THROW_EXCEPTION( className, MEMORY_ERROR );
                }
            }
            else
            {
                THROW_EXCEPTION( className, DIMENSION_OVERFLOW );
            }

            break;
        }
    }


    /**
    *  Instantiates a \c Matrix class to the size indicated.\n
    *  The matrix can be instantiated through a macro as follows:
    *  \code Matrix(A,double,unsigned int,3) {21,65,10} End; \endcode
    *  If a pointer to the instantiated matrix is desired, the following macro can be used:
    *  \code MatrixPtr(A,double,unsigned int,3) {21,65,10} End; \endcode
    *  In the last case, remember to free allocated memory through:
    *  \code delete A; \endcode
    *  \version 1.2.7a
    *  \date    31 January 2011
    *  \param [in] size Array of \c LengthType values, indicating the length of each dimension.
    *  \throws  MEMORY_ERROR when constructor cannot allocate enough memory for the \c Matrix class.
    *  \throws  LENGTH_OVERFLOW when required length exceeds the maximum value storable in a LengthType variable.
    */
    template <class DataType, typename LengthType, unsigned char dimensions>
            Matrix<DataType,LengthType,dimensions>::Matrix(const LengthType (&input_size)[dimensions]) : square(false), row(0),
                                                                                                         length(1),
                                                                                                         className( MATRIX_NAME )
    {
        if( dimensions < 8 * sizeof( LengthType ) )
        {
            // Local initializations
            LengthType max_length = ( LengthType )2 << ( sizeof( LengthType ) * 8 - 2 );
            max_length += max_length - 1;

            // Allocate size vector
            size = new (nothrow) LengthType[dimensions];

            if( size == 0 )
            {
                THROW_EXCEPTION( className, MEMORY_ERROR );
            }

            for( i = 0; i < dimensions; i++ )
            {
                // Copy the value of each dimension from input vector
                size[i] = input_size[i];

                // Check for length overflow
                if( length <= max_length / size[i] )
                    length = length * size[i];
                else
                {
                    delete [] size;
                    THROW_EXCEPTION( className, "Maximum length exceeded." );
                }
            }

            // Set square matrix flag
            if( dimensions == 2 )
            {
                square = ( size[0] == size[1] );
            }

            // Allocate memory for temporary row data storage
            if( ( dimensions == 1 ) || ( dimensions == 2 ) )
            {
                if( dimensions == 1 )
                {
                    row = new (nothrow) DataType;
                }
                else if( dimensions == 2 )
                {
                    row = new (nothrow) DataType[ size[1] ];
                }

                if( row == 0 )
                {
                    THROW_EXCEPTION( className, MEMORY_ERROR );
                }
            }

            // Allocate memory for matrix data
            data = new (nothrow) DataType[length];

            if( data == 0 )
            {
                delete [] size;
                THROW_EXCEPTION( className, MEMORY_ERROR );
            }

            // Set data to zero
            memset( data, 0, length * sizeof( DataType ) );
        }
        else
        {
            THROW_EXCEPTION( className, DIMENSION_OVERFLOW );
        }
    }


    /**
    *  Instantiates a \c Matrix class that is the copy of \e m.
    *  \version 1.1.5a
    *  \date    31 January 2011
    *  \param [in] m Pointer to a \c Matrix class.
    *  \throws  MEMORY_ERROR when constructor cannot allocate enough memory for the \c Matrix class.
    */
    template <class DataType, typename LengthType, unsigned char dimensions>
            Matrix<DataType,LengthType,dimensions>::Matrix(const Matrix &m) : row(0), length(0), className( MATRIX_NAME )
    {
        if( dimensions > 0 )
        {
            // Create size vector
            size = new (nothrow) LengthType[dimensions];

            if( size == 0 )
            {
                THROW_EXCEPTION( className, MEMORY_ERROR );
            }

            copy( m );
        }
    }


    /**
    *  Frees all the resources allocated for the class.
    *  \version 1.1.6a
    *  \date    28 January 2011
    */
    template <class DataType, typename LengthType, unsigned char dimensions>
            Matrix<DataType,LengthType,dimensions>::~Matrix()
    {
        if( ( dimensions > 0 ) && ( dimensions < 8 * sizeof( LengthType ) ) )
        {
            if( length > 0 )
            {
                delete [] data;
            }

            delete [] size;
        }

        if( dimensions == 1 )
        {
            if( row != 0 )
            {
                delete row;
            }
        }
        else if( dimensions == 2 )
        {
            if( row != 0 )
            {
                delete [] row;
            }
        }
    }


    //***Accessors***

    /**
    *  Provides a public interface to get matrix dimensions.
    *  \version 1.0.0a
    *  \date    9 August 2010
    *  \returns The number of dimensions of \c this \c Matrix class.
    */
    template <class DataType, typename LengthType, unsigned char dimensions>
            unsigned char Matrix<DataType,LengthType,dimensions>::getDimensions()
    {
        return dimensions;
    }


    /**
    *  Provides a public interface to get matrix length.
    *  \version 1.0.0a
    *  \date    2 September 2010
    *  \returns The number of data elements of \c this \c Matrix class.
    */
    template <class DataType, typename LengthType, unsigned char dimensions>
            LengthType Matrix<DataType,LengthType,dimensions>::getLength()
    {
        return length;
    }


    /**
    *  Provides a public interface to get the size of the specified matrix dimension.
    *  \version 1.0.1a
    *  \date    6 September 2010
    *  \param [in] dimension The number of the dimension for which the size is requested (starting from 0).
    *  \returns The size of the specified dimension of \c this \c Matrix class.
    *  \throws  NO_SIZE_AVAILABLE when matrix has 0 dimensions.
    *  \throws  DIM_INDEX_OVERFLOW when the specified \e dimension is greater or equal to the matrix dimensions (only in DEBUG mode).
    */
    template <class DataType, typename LengthType, unsigned char dimensions>
            LengthType Matrix<DataType,LengthType,dimensions>::getSize( unsigned char dimension )
    {
        if( dimensions > 0 )
        {
#ifdef DEBUG
            if( dimension < dimensions )
            {
#endif
                return size[dimension];
#ifdef DEBUG
            }
            else
            {
                THROW_EXCEPTION( className, "Index exceeds matrix dimensions." );
            }
#endif
        }
        else
        {
            THROW_EXCEPTION( className, "Matrix with 0 dimensions has no size." );
        }
    }


    //***Operators overloading***

    /**
    *  Provides the reference to the \c i-th data element.
    *  \version 1.0.1a
    *  \date    6 September 2010
    *  \param [in] i Index of selected element.
    *  \returns The reference to the selected data element.
    *  \throws  NO_DATA_AVAILABLE when matrix has 0 length (only in DEBUG mode).
    *  \throws  LEN_INDEX_OVERFLOW when the specified \e index is greater or equal to the matrix length (only in DEBUG mode).
    */
    template <class DataType, typename LengthType, unsigned char dimensions>
            DataType& Matrix<DataType,LengthType,dimensions>::operator() ( const LengthType& i )
    {
#ifdef DEBUG
        if( length > 0 )
        {
            if( i < length )
            {
#endif
                return data[i];
#ifdef DEBUG
            }
            else
            {
                THROW_EXCEPTION( className, LEN_INDEX_OVERFLOW );
            }
        }
        else
        {
            THROW_EXCEPTION( className, NO_DATA_AVAILABLE );
        }
#endif
    }


    /**
    *  Provides the reference to the \c i-th row and j-th column data element of a 2D matrix.\n
    *  The data elements are disposed in memory as a sequence of matrix rows.
    *  \version 1.0.1a
    *  \date    6 September 2010
    *  \param [in] i Index of selected row.
    *  \param [in] j Index of selected column.
    *  \returns The reference to the selected data element.
    *  \throws  WRONG_NUMBER_OF_INDICES when the operator is called for a non-2D matrix.
    *  \throws  NO_DATA_AVAILABLE when matrix has 0 length (only in DEBUG mode).
    *  \throws  LEN_INDEX_OVERFLOW when the specified \e indices exceed matrix length (only in DEBUG mode).
    */
    template <class DataType, typename LengthType, unsigned char dimensions>
            DataType& Matrix<DataType,LengthType,dimensions>::operator() ( const LengthType& i, const LengthType& j )
    {
        if( dimensions == 2 )
        {
#ifdef DEBUG
            if( length > 0 )
            {
                if( ( i < size[0] ) && ( j < size[1] ) )
                {
#endif
                    return data[ size[1] * i + j ];
#ifdef DEBUG
                }
                else
                {
                    THROW_EXCEPTION( className, LEN_INDEX_OVERFLOW );
                }
            }
            else
            {
                THROW_EXCEPTION( className, NO_DATA_AVAILABLE );
            }
#endif
        }
        else
        {
            THROW_EXCEPTION( className, "Wrong number of indices." );
        }
    }


    /**
    *  Performs the fast assignment operation, copying the contents of \e right_hand into \c this matrix, if they have the same size.
    *  \version 1.2.1a
    *  \date    15 September 2010
    *  \param [in] right_hand Right hand \c Matrix class.
    *  \returns \c this \c Matrix class.
    *  \throws  SIZE_MISMATCH when matrices size does not agree (only in DEBUG mode).
    */
    template <class DataType, typename LengthType, unsigned char dimensions>
            Matrix<DataType,LengthType,dimensions>& Matrix<DataType,LengthType,dimensions>::operator= (const Matrix &right_hand)
    {
        if( dimensions > 0 )
        {
#ifdef DEBUG
            checkSizeMismatch( right_hand );
#endif

            // Assign data
            for( i = 0; i < length; i++ )
                data[i] = right_hand.data[i];
        }

        return *this;
    }


    /**
    *  Assigns the specified value to each data element of \c this matrix.
    *  \version 1.0.0a
    *  \date    6 September 2010
    *  \param [in] right_hand Right hand \c DataType value.
    *  \returns \c this \c Matrix class.
    */
    template <class DataType, typename LengthType, unsigned char dimensions>
            Matrix<DataType,LengthType,dimensions>& Matrix<DataType,LengthType,dimensions>::operator= (const DataType &right_hand)
    {
        if( dimensions > 0 )
        {
            // Assign data
            for( i = 0; i < length; i++ )
                data[i] = right_hand;
        }

        return *this;
    }


    /**
    *  Performs the addition operation, adding each data value of right hand matrix to the corresponding data value of \c this matrix.
    *  \version 1.0.1a
    *  \date    15 September 2010
    *  \param [in] right_hand Right hand \c Matrix class.
    *  \returns \c this \c Matrix class.
    *  \throws  SIZE_MISMATCH when matrices size does not agree (only in DEBUG mode).
    */
    template <class DataType, typename LengthType, unsigned char dimensions>
            Matrix<DataType,LengthType,dimensions>& Matrix<DataType,LengthType,dimensions>::operator+= (const Matrix &right_hand)
    {
        if( dimensions > 0 )
        {
#ifdef DEBUG
            checkSizeMismatch( right_hand );
#endif

            for( i = 0; i < length; i++ )
                data[i] += right_hand.data[i];
        }

        return *this;
    }


    /**
    *  Performs the addition operation, adding the value of right hand DataType to each data value of \c this matrix.
    *  \version 1.0.0a
    *  \date    14 September 2010
    *  \param [in] right_hand Right hand \c DataType value.
    *  \returns \c this \c Matrix class.
    */
    template <class DataType, typename LengthType, unsigned char dimensions>
            Matrix<DataType,LengthType,dimensions>& Matrix<DataType,LengthType,dimensions>::operator+= (const DataType &right_hand)
    {
        if( dimensions > 0 )
        {
            for( i = 0; i < length; i++ )
                data[i] += right_hand;
        }

        return *this;
    }


    /**
    *  Performs the element-wise sum of \c this matrix and \e right_hand.
    *  \version 1.2.0a
    *  \date    14 September 2010
    *  \param [in] right_hand Right hand \c Matrix class.
    *  \returns A \c Matrix class containing the result of the operation.
    *  \throws  SIZE_MISMATCH when matrices size does not agree (only in DEBUG mode).
    */
    template <class DataType, typename LengthType, unsigned char dimensions>
            Matrix<DataType,LengthType,dimensions> Matrix<DataType,LengthType,dimensions>::operator+ (const Matrix &right_hand)
    {
        Matrix<DataType,LengthType,dimensions> result( *this );
        result += right_hand;
        return result;
    }


    /**
    *  Performs the element-wise sum of each matrix element with a scalar.
    *  \version 1.2.0a
    *  \date    14 September 2010
    *  \param [in] right_hand Right hand \c DataType value.
    *  \returns A \c Matrix class containing the result of the operation.
    */
    template <class DataType, typename LengthType, unsigned char dimensions>
            Matrix<DataType,LengthType,dimensions> Matrix<DataType,LengthType,dimensions>::operator+ (const DataType &right_hand)
    {
        Matrix<DataType,LengthType,dimensions> result( *this );
        result += right_hand;
        return result;
    }


    /**
    *  Performs the subtraction operation, subtracting each data value of right hand matrix to the corresponding data value of
    *  \c this matrix.
    *  \version 1.0.0a
    *  \date    15 September 2010
    *  \param [in] right_hand Right hand \c Matrix class.
    *  \returns \c this \c Matrix class.
    *  \throws  SIZE_MISMATCH when matrices size does not agree (only in DEBUG mode).
    */
    template <class DataType, typename LengthType, unsigned char dimensions>
            Matrix<DataType,LengthType,dimensions>& Matrix<DataType,LengthType,dimensions>::operator-= (const Matrix &right_hand)
    {
        if( dimensions > 0 )
        {
#ifdef DEBUG
            checkSizeMismatch( right_hand );
#endif

            for( i = 0; i < length; i++ )
                data[i] -= right_hand.data[i];
        }

        return *this;
    }


    /**
    *  Performs the subtraction operation, subtracting the value of right hand DataType to each data value of \c this matrix.
    *  \version 1.0.0a
    *  \date    15 September 2010
    *  \param [in] right_hand Right hand \c DataType value.
    *  \returns \c this \c Matrix class.
    */
    template <class DataType, typename LengthType, unsigned char dimensions>
            Matrix<DataType,LengthType,dimensions>& Matrix<DataType,LengthType,dimensions>::operator-= (const DataType &right_hand)
    {
        if( dimensions > 0 )
        {
            for( i = 0; i < length; i++ )
                data[i] -= right_hand;
        }

        return *this;
    }


    /**
    *  Performs the element-wise difference of \c this matrix and \e right_hand.
    *  \version 1.2.0a
    *  \date    15 September 2010
    *  \param [in] right_hand Right hand \c Matrix class.
    *  \returns A \c Matrix class containing the result of the operation.
    *  \throws  SIZE_MISMATCH when matrices size does not agree (only in DEBUG mode).
    */
    template <class DataType, typename LengthType, unsigned char dimensions>
            Matrix<DataType,LengthType,dimensions> Matrix<DataType,LengthType,dimensions>::operator- (const Matrix &right_hand)
    {
        Matrix<DataType,LengthType,dimensions> result( *this );
        result -= right_hand;
        return result;
    }


    /**
    *  Performs the element-wise difference of each matrix element with a scalar.
    *  \version 1.2.0a
    *  \date    15 September 2010
    *  \param [in] right_hand Right hand \c DataType value.
    *  \returns A \c Matrix class containing the result of the operation.
    */
    template <class DataType, typename LengthType, unsigned char dimensions>
            Matrix<DataType,LengthType,dimensions> Matrix<DataType,LengthType,dimensions>::operator- (const DataType &right_hand)
    {
        Matrix<DataType,LengthType,dimensions> result( *this );
        result -= right_hand;
        return result;
    }


    /**
    *  Performs the row by column multiplication of \c this 2D matrix by the 2D right hand matrix.\n
    *  This method provides the fastest way to execute a matrix multiplication, because it does not allocate memory for a new array,
    *  but it modifies existing one, only if needed. If a matrix product is needed to be assigned to a third pre-allocated matrix, in
    *  the fastest way possible, instead of using:
    *  \code C = A * B; \endcode
    *  is fastest to use:
    *  \code C = A;
    * C *= B; \endcode
    *  \version 1.0.4a
    *  \date    28 January 2011
    *  \param [in] right_hand Right hand \c Matrix class.
    *  \returns \c this \c Matrix class.
    *  \throws  DIMENSIONS_MISMATCH when the result of multiplication has a 2 dimensions, while \c this matrix has 1 dimension.
    *  \throws  MEMORY_ERROR when constructor cannot allocate enough memory for the \c Matrix class.
    *  \throws  OPERATION_NOT_SUPPORTED when \c this matrix has more than 2 dimensions.
    *  \throws  INNER_DIM_MISMATCH when the inner dimensions of matrices do not agree (only in DEBUG mode).
    */
    template <class DataType, typename LengthType, unsigned char dimensions>
            Matrix<DataType,LengthType,2>& Matrix<DataType,LengthType,dimensions>::operator*=
            (const Matrix<DataType,LengthType,2> &right_hand)
    {
        if( dimensions == 2 )
        {
#ifdef DEBUG
            if( size[1] != right_hand.size[0] )
            {
                THROW_EXCEPTION( className, INNER_DIM_MISMATCH );
            }
#endif

            // Update length
            length = size[0] * right_hand.size[1];

            // Increment memory allocation for data if necessary
            bool increment = ( size[1] < right_hand.size[1] );

            if( increment )
            {
                data = ( DataType* )realloc( data, length * sizeof( DataType ) );

                if( data == 0 )
                {
                    THROW_EXCEPTION( className, MEMORY_ERROR );
                }

                row = ( DataType* )realloc( row, right_hand.size[1] * sizeof( DataType ) );

                if( row == 0 )
                {
                    THROW_EXCEPTION( className, MEMORY_ERROR );
                }
            }

            // Update matrix inner size and store old value
            LengthType max_j = size[1];
            size[1] = right_hand.size[1];

            // Redistribute data, if size increased
            if( increment )
            {
                for( i = size[0] - 1; i > 0; i-- )
                {
                    for( j = max_j - 1; j <= max_j - 1; j-- )
                    {
                        ( *this )(i,j) = data[ max_j * i + j ];
                    }
                }
            }

            for( i = 0; i < size[0]; i++ )
            {
                // Copy data row to temporary row
                for( j = 0; j < max_j; j++ )
                {
                    switch( increment )
                    {
                        case true:
                            row[j] = ( *this )(i,j);
                            break;

                        case false:
                            row[j] = data[ max_j * i + j ];
                            break;
                    }
                }

                for( k = 0; k < right_hand.size[1]; k++ )
                {
                    // Initialize to 0 the output data element
                    ( *this )(i,k) = 0;

                    // Calculate element (i,k)
                    for( j = 0; j < max_j; j++ )
                    {
                        ( *this )(i,k) += row[j] * ( ( Matrix )right_hand )(j,k);
                    }
                }

                row[0] = row[max_j];
            }

            // Reduce memory allocation for data if necessary
            if( max_j > right_hand.size[1] )
            {
                data = ( DataType* )realloc( data, length * sizeof( DataType ) );

                if( data == 0 )
                {
                    THROW_EXCEPTION( className, MEMORY_ERROR );
                }

                row = ( DataType* )realloc( row, right_hand.size[1] * sizeof( DataType ) );

                if( row == 0 )
                {
                    THROW_EXCEPTION( className, MEMORY_ERROR );
                }
            }

            // Set square matrix flag
            square = ( size[0] == size[1] );
        }
        else if( dimensions == 1 )
        {
            THROW_EXCEPTION( className, "Cannot modify matrix dimensions." );
        }
        else
        {
            THROW_EXCEPTION( className, OPERATION_NOT_SUPPORTED );
        }

        return *this;
    }


    /**
    *  Performs the multiplication of each element of \c this matrix by the right hand DataType.
    *  \version 1.0.0a
    *  \date    24 January 2011
    *  \param [in] right_hand Right hand \c Matrix class.
    *  \returns \c this \c Matrix class.
    */
    template <class DataType, typename LengthType, unsigned char dimensions>
            Matrix<DataType,LengthType,dimensions>& Matrix<DataType,LengthType,dimensions>::operator*= (const DataType &right_hand)
    {
        if( dimensions > 0 )
        {
            for( i = 0; i < length; i++ )
                data[i] *= right_hand;
        }

        return *this;
    }


    /**
    *  Performs the row by column product of \c this matrix by the 2D right hand matrix.\n
    *  \version 1.0.1a
    *  \date    28 January 2011
    *  \param [in] right_hand Right hand 2D \c Matrix class.
    *  \returns A 2D \c Matrix class containing the result of the operation.
    *  \throws  OPERATION_NOT_SUPPORTED when \c this matrix has more than 2 dimensions.
    *  \throws  INNER_DIM_MISMATCH when the inner dimensions of matrices do not agree (only in DEBUG mode).
    */
    template <class DataType, typename LengthType, unsigned char dimensions>
            Matrix<DataType,LengthType,2> Matrix<DataType,LengthType,dimensions>::operator*
            (const Matrix<DataType,LengthType,2> &right_hand)
    {
#define right_hand  ( ( Matrix<DataType,LengthType,2>& )right_hand )

#ifdef DEBUG
        // Check inner size
        if( dimensions == 2 )
        {
            if( size[1] != right_hand.getSize(0) )
            {
                THROW_EXCEPTION( className, INNER_DIM_MISMATCH );
            }
        }
        else if( dimensions == 1 )
        {
            if( right_hand.getSize(0) != 1 )
            {
                THROW_EXCEPTION( className, INNER_DIM_MISMATCH );
            }
        }
#endif

        const LengthType outSize[2] = {size[0],right_hand.getSize(1)};
        Matrix<DataType,LengthType,2> result( outSize );

        if( dimensions == 2 )
        {
            for( i = 0; i < size[0]; i++ )
            {
                for( k = 0; k < right_hand.getSize(1); k++ )
                {
                    // Initialize to 0 the output data element
                    result(i,k) = 0;

                    // Calculate element (i,k)
                    for( j = 0; j < size[1]; j++ )
                    {
                        result(i,k) += ( *this )(i,j) * right_hand(j,k);
                    }
                }
            }
        }
        else if( dimensions == 1 )
        {
            for( i = 0; i < length; i++ )
            {
                for( k = 0; k < right_hand.getSize(1); k++ )
                {
                    // Calculate element (i,k)
                    result(i,k) = data[i] * right_hand(k);
                }
            }
        }
        else
        {
            THROW_EXCEPTION( className, OPERATION_NOT_SUPPORTED );
        }

        return result;

#undef right_hand
    }


    /**
    *  Performs the row by column product of \c this matrix by the 1D right hand matrix.\n
    *  \version 1.0.1a
    *  \date    28 January 2011
    *  \param [in] right_hand Right hand 1D \c Matrix class.
    *  \returns A 1D \c Matrix class containing the result of the operation.
    *  \throws  OPERATION_NOT_SUPPORTED when \c this matrix has more than 2 dimensions.
    *  \throws  INNER_DIM_MISMATCH when the inner dimensions of matrices do not agree (only in DEBUG mode).
    */
    template <class DataType, typename LengthType, unsigned char dimensions>
            Matrix<DataType,LengthType,1> Matrix<DataType,LengthType,dimensions>::operator*
            (const Matrix<DataType,LengthType,1> &right_hand)
    {
#define right_hand  ( ( Matrix<DataType,LengthType,1>& )right_hand )

#ifdef DEBUG
        // Check inner size
        if( dimensions == 2 )
        {
            if( size[1] != right_hand.getSize(0) )
            {
                THROW_EXCEPTION( className, INNER_DIM_MISMATCH );
            }
        }
        else if( dimensions == 1 )
        {
            if( right_hand.getSize(0) != 1 )
            {
                THROW_EXCEPTION( className, INNER_DIM_MISMATCH );
            }
        }
#endif

        const LengthType outSize[1] = {size[0]};
        Matrix<DataType,LengthType,1> result( outSize );

        if( dimensions == 2 )
        {
            for( i = 0; i < size[0]; i++ )
            {
                // Initialize to 0 the output data element
                result(i) = 0;

                // Calculate element (i)
                for( j = 0; j < size[1]; j++ )
                {
                    result(i) += ( *this )(i,j) * right_hand(j);
                }
            }
        }
        else if( dimensions == 1 )
        {
            for( i = 0; i < length; i++ )
            {
                // Calculate element (i)
                result(i) = data[i] * right_hand(0);
            }
        }
        else
        {
            THROW_EXCEPTION( className, OPERATION_NOT_SUPPORTED );
        }

        return result;

#undef right_hand
    }


    /**
    *  Performs the multiplication of each element of \c this matrix by the right hand DataType.
    *  \version 1.1.1a
    *  \date    28 January 2011
    *  \param [in] right_hand Right hand \c Matrix class.
    *  \returns A \c Matrix class containing the result of the operation.
    */
    template <class DataType, typename LengthType, unsigned char dimensions>
            Matrix<DataType,LengthType,dimensions> Matrix<DataType,LengthType,dimensions>::operator* (const DataType &right_hand)
    {
        Matrix<DataType,LengthType,dimensions> result( *this );
        return result *= right_hand;
    }


    /**
    *  Performs the element-wise equality check operation between \c this matrix and right hand one.
    *  \version 1.2.0a
    *  \date    4 February 2011
    *  \param [in] right_hand Right hand \c Matrix class.
    *  \returns a \c Matrix with \c bool DataType, whose elements are the result of element-wise equality comparison.
    *  \throws  SIZE_MISMATCH when matrices size does not agree (only in DEBUG mode).
    */
    template <class DataType, typename LengthType, unsigned char dimensions>
            Matrix<bool,LengthType,dimensions> Matrix<DataType,LengthType,dimensions>::operator== (const Matrix &right_hand)
    {
        return comparison( right_hand, EQUAL_TO );
    }


    /**
    *  Performs the element-wise equality check operation between \c this matrix and a right hand scalar value.
    *  \version 1.0.0a
    *  \date    8 February 2011
    *  \param [in] right_hand Right hand \c DataType value.
    *  \returns a \c Matrix with \c bool DataType, whose elements are the result of element-wise equality comparison.
    */
    template <class DataType, typename LengthType, unsigned char dimensions>
            Matrix<bool,LengthType,dimensions> Matrix<DataType,LengthType,dimensions>::operator== (const DataType &right_hand)
    {
        return comparison( right_hand, EQUAL_TO );
    }


    /**
    *  Performs the element-wise inequality check operation between \c this matrix and right hand one.
    *  \version 1.1.0a
    *  \date    7 February 2011
    *  \param [in] right_hand Right hand \c Matrix class.
    *  \returns a \c Matrix with \c bool DataType, whose elements are the result of element-wise inequality comparison.
    *  \throws  SIZE_MISMATCH when matrices size does not agree (only in DEBUG mode).
    */
    template <class DataType, typename LengthType, unsigned char dimensions>
            Matrix<bool,LengthType,dimensions> Matrix<DataType,LengthType,dimensions>::operator!= (const Matrix &right_hand)
    {
        return comparison( right_hand, NOT_EQUAL_TO );
    }


    /**
    *  Performs the element-wise inequality check operation between \c this matrix and a right hand scalar value.
    *  \version 1.0.0a
    *  \date    8 February 2011
    *  \param [in] right_hand Right hand \c DataType value.
    *  \returns a \c Matrix with \c bool DataType, whose elements are the result of element-wise inequality comparison.
    */
    template <class DataType, typename LengthType, unsigned char dimensions>
            Matrix<bool,LengthType,dimensions> Matrix<DataType,LengthType,dimensions>::operator!= (const DataType &right_hand)
    {
        return comparison( right_hand, NOT_EQUAL_TO );
    }


    /**
    *  Performs the element-wise greater than comparison operation between \c this matrix and right hand one.
    *  \version 1.0.0a
    *  \date    7 February 2011
    *  \param [in] right_hand Right hand \c Matrix class.
    *  \returns a \c Matrix with \c bool DataType, whose elements are the result of element-wise greater than comparison.
    *  \throws  SIZE_MISMATCH when matrices size does not agree (only in DEBUG mode).
    */
    template <class DataType, typename LengthType, unsigned char dimensions>
            Matrix<bool,LengthType,dimensions> Matrix<DataType,LengthType,dimensions>::operator> (const Matrix &right_hand)
    {
        return comparison( right_hand, GREATER_THAN );
    }


    /**
    *  Performs the element-wise greater than comparison operation between \c this matrix and a right hand scalar value.
    *  \version 1.0.0a
    *  \date    8 February 2011
    *  \param [in] right_hand Right hand \c DataType value.
    *  \returns a \c Matrix with \c bool DataType, whose elements are the result of element-wise greater than comparison.
    */
    template <class DataType, typename LengthType, unsigned char dimensions>
            Matrix<bool,LengthType,dimensions> Matrix<DataType,LengthType,dimensions>::operator> (const DataType &right_hand)
    {
        return comparison( right_hand, GREATER_THAN );
    }


    /**
    *  Performs the element-wise greater than or equal to comparison operation between \c this matrix and right hand one.
    *  \version 1.0.0a
    *  \date    7 February 2011
    *  \param [in] right_hand Right hand \c Matrix class.
    *  \returns a \c Matrix with \c bool DataType, whose elements are the result of element-wise greater than or equal to comparison.
    *  \throws  SIZE_MISMATCH when matrices size does not agree (only in DEBUG mode).
    */
    template <class DataType, typename LengthType, unsigned char dimensions>
            Matrix<bool,LengthType,dimensions> Matrix<DataType,LengthType,dimensions>::operator>= (const Matrix &right_hand)
    {
        return comparison( right_hand, GREATER_THAN_EQUAL_TO );
    }


    /**
    *  Performs the element-wise greater than or equal to comparison operation between \c this matrix and a right hand scalar value.
    *  \version 1.0.0a
    *  \date    8 February 2011
    *  \param [in] right_hand Right hand \c DataType value.
    *  \returns a \c Matrix with \c bool DataType, whose elements are the result of element-wise greater than comparison.
    */
    template <class DataType, typename LengthType, unsigned char dimensions>
            Matrix<bool,LengthType,dimensions> Matrix<DataType,LengthType,dimensions>::operator>= (const DataType &right_hand)
    {
        return comparison( right_hand, GREATER_THAN_EQUAL_TO );
    }


    /**
    *  Performs the element-wise lower than comparison operation between \c this matrix and right hand one.
    *  \version 1.0.0a
    *  \date    7 February 2011
    *  \param [in] right_hand Right hand \c Matrix class.
    *  \returns a \c Matrix with \c bool DataType, whose elements are the result of element-wise lower than comparison.
    *  \throws  SIZE_MISMATCH when matrices size does not agree (only in DEBUG mode).
    */
    template <class DataType, typename LengthType, unsigned char dimensions>
            Matrix<bool,LengthType,dimensions> Matrix<DataType,LengthType,dimensions>::operator< (const Matrix &right_hand)
    {
        return comparison( right_hand, LOWER_THAN );
    }


    /**
    *  Performs the element-wise lower than comparison operation between \c this matrix and a right hand scalar value.
    *  \version 1.0.0a
    *  \date    8 February 2011
    *  \param [in] right_hand Right hand \c DataType value.
    *  \returns a \c Matrix with \c bool DataType, whose elements are the result of element-wise lower than comparison.
    */
    template <class DataType, typename LengthType, unsigned char dimensions>
            Matrix<bool,LengthType,dimensions> Matrix<DataType,LengthType,dimensions>::operator< (const DataType &right_hand)
    {
        return comparison( right_hand, LOWER_THAN );
    }


    /**
    *  Performs the element-wise lower than or equal to comparison operation between \c this matrix and right hand one.
    *  \version 1.0.0a
    *  \date    7 February 2011
    *  \param [in] right_hand Right hand \c Matrix class.
    *  \returns a \c Matrix with \c bool DataType, whose elements are the result of element-wise lower than or equal to comparison.
    *  \throws  SIZE_MISMATCH when matrices size does not agree (only in DEBUG mode).
    */
    template <class DataType, typename LengthType, unsigned char dimensions>
            Matrix<bool,LengthType,dimensions> Matrix<DataType,LengthType,dimensions>::operator<= (const Matrix &right_hand)
    {
        return comparison( right_hand, LOWER_THAN_EQUAL_TO );
    }


    /**
    *  Performs the element-wise lower than or equal to comparison operation between \c this matrix and a right hand scalar value.
    *  \version 1.0.0a
    *  \date    8 February 2011
    *  \param [in] right_hand Right hand \c DataType value.
    *  \returns a \c Matrix with \c bool DataType, whose elements are the result of element-wise lower than or equal to comparison.
    */
    template <class DataType, typename LengthType, unsigned char dimensions>
            Matrix<bool,LengthType,dimensions> Matrix<DataType,LengthType,dimensions>::operator<= (const DataType &right_hand)
    {
        return comparison( right_hand, LOWER_THAN_EQUAL_TO );
    }


    /**
    *  Performs the copy of \c this matrix to the indicated one, modifying the size of the latter, if necessary.
    *  \version 1.0.2a
    *  \date    31 January 2011
    *  \param [in] right_hand Target \c Matrix class.
    *  \returns \c this \c Matrix class.
    *  \throws  MEMORY_ERROR when operator cannot allocate enough memory for the \c Matrix class.
    */
    template <class DataType, typename LengthType, unsigned char dimensions>
            Matrix<DataType,LengthType,dimensions>& Matrix<DataType,LengthType,dimensions>::copy( const Matrix &right_hand )
    {
        if (dimensions > 0)
        {
            // Check for self-assignment
            if (this == &right_hand)
            {
                return *this;
            }

            if( length == 0 )
            {
                if( right_hand.length == 0 )
                {
                    // Nothing to be done
                    return *this;
                }
                else
                {
                    // Allocate memory for matrix data
                    data = new (nothrow) DataType[right_hand.length];

                    if( data == 0 )
                    {
                        delete [] size;
                        THROW_EXCEPTION( className, MEMORY_ERROR );
                    }

                    // Allocate memory for temporary row
                    if( ( dimensions == 1 ) || ( dimensions == 2 ) )
                    {
                        if( dimensions == 1 )
                        {
                            row = new (nothrow) DataType;
                        }
                        else if( dimensions == 2 )
                        {
                            row = new (nothrow) DataType[ right_hand.size[1] ];
                        }

                        if( row == 0 )
                        {
                            delete [] size;
                            delete [] data;
                            THROW_EXCEPTION( className, MEMORY_ERROR );
                        }
                    }
                }
            }
            else
            {
                if( right_hand.length == 0 )
                {
                    // Free memory for matrix data
                    delete [] data;

                    // Free memory for temporary row
                    if( dimensions == 1 )
                    {
                        delete row;
                        row = 0;
                    }
                    else if( dimensions == 2 )
                    {
                        delete [] row;
                        row = 0;
                    }
                }
                else
                {
                    if( length != right_hand.length )
                    {
                        // Reallocate memory for matrix data
                        data = ( DataType* )realloc( data, right_hand.length * sizeof( DataType ) );

                        if( data == 0 )
                        {
                            delete [] size;
                            THROW_EXCEPTION( className, MEMORY_ERROR );
                        }
                    }

                    if( dimensions == 2 )
                    {
                        if( size[1] != right_hand.size[1] )
                        {
                            // Reallocate memory for temporary row
                            row = ( DataType* )realloc( row, right_hand.size[1] * sizeof( DataType ) );

                            if( row == 0 )
                            {
                                delete [] size;
                                delete [] data;
                                THROW_EXCEPTION( className, MEMORY_ERROR );
                            }
                        }
                    }
                }
            }

            // Assign matrix parameters
            square = right_hand.square;
            length = right_hand.length;

            for( i = 0; i < dimensions; i++ )
                size[i] = right_hand.size[i];

            // Assign data
            for( i = 0; i < length; i++ )
                data[i] = right_hand.data[i];
        }

        return *this;
    }


    //***Property accessors***

    /**
    *  Performs the element-wise equality check operation between \c this matrix and right hand one.
    *  \version 1.0.0a
    *  \date    4 February 2011
    *  \param [in] right_hand Right hand \c Matrix class.
    *  \returns \c true if the size of each dimension and each pair of corresponding elements of the two matrices are equal and
    *  \c false otherwise.
    */
    template <class DataType, typename LengthType, unsigned char dimensions>
            bool Matrix<DataType,LengthType,dimensions>::isEqualTo(const Matrix &right_hand)
    {
        for( i = 0; i < dimensions; i++ )
        {
            if( size[i] != right_hand.size[i] )
            {
                return false;
            }
        }

        for( i = 0; i < length; i++ )
        {
            if( data[i] != right_hand.data[i] )
            {
                return false;
            }
        }

        return true;
    }


    /**
    *  Tells if \c this Matrix is a 2D square matrix.
    *  \version 1.0.0a
    *  \date    28 January 2011
    *  \returns \c true if the matrix is square, \c false otherwise.
    */
    template <class DataType, typename LengthType, unsigned char dimensions>
            bool Matrix<DataType,LengthType,dimensions>::isSquare()
    {
        return square;
    }


    /**
    *  Tells if \c this Matrix is a 2D square symmetric matrix.
    *  \version 1.0.0a
    *  \date    1 February 2011
    *  \returns \c true if the matrix is symmetric, \c false otherwise.
    *  \throws  MATRIX_NOT_SQUARE when \c this is not a square matrix.
    */
    template <class DataType, typename LengthType, unsigned char dimensions>
            bool Matrix<DataType,LengthType,dimensions>::isSymmetric()
    {
        // Check square matrix
#ifdef DEBUG
        if( !isSquare() )
        {
            THROW_EXCEPTION( className, MATRIX_NOT_SQUARE );
        }
#endif

        for( i = 0; i < size[0] - 1; i++ )
        {
            for( j = i + 1; j < size[1]; j++ )
            {
                if( ( *this )(i,j) != ( *this )(j,i) )
                {
                    return false;
                }
            }
        }

        return true;
    }


    //***Linear Algebra***

    /**
    *  Calculates the trace (sum of the diagonal elements) of \c this matrix.
    *  \version 1.1.2a
    *  \date    31 January 2011
    *  \returns The value of the trace of \c this \c Matrix class.
    *  \throws  OPERATION_NOT_SUPPORTED when \c this matrix has not 2 dimensions.
    *  \throws  MATRIX_NOT_SQUARE when \c this is not a square matrix.
    */
    template <class DataType, typename LengthType, unsigned char dimensions>
            DataType Matrix<DataType,LengthType,dimensions>::trace()
    {
        if( dimensions == 2 )
        {
#ifdef DEBUG
            // Check square matrix
            if( !isSquare() )
            {
                THROW_EXCEPTION( className, MATRIX_NOT_SQUARE );
            }
#endif

            DataType result = data[0];

            for( i = 1; i < size[0]; i++ )
            {
                result += ( *this )(i,i);
            }

            return result;
        }
        else
        {
            THROW_EXCEPTION( className, OPERATION_NOT_SUPPORTED );
        }
    }


    /**
    *  Extracts the diagonal elements of \c this 2D square \c Matrix class and puts them in a 1D \c Matrix.
    *  \version 1.2.0a
    *  \date    31 January 2011
    *  \returns 1D \c Matrix containing the diagonal elements of \c this 2D square \c Matrix class.
    *  \throws  OPERATION_NOT_SUPPORTED when \c this matrix has not 2 dimensions.
    *  \throws  MATRIX_NOT_SQUARE when \c this is not a square matrix.
    */
    template <class DataType, typename LengthType, unsigned char dimensions>
            Matrix<DataType,LengthType,1> Matrix<DataType,LengthType,dimensions>::diag()
    {
        if( dimensions == 2 )
        {
#ifdef DEBUG
            // Check square matrix
            if( !isSquare() )
            {
                THROW_EXCEPTION( className, MATRIX_NOT_SQUARE );
            }
#endif

            const LengthType outSize[1] = {size[0]};
            Matrix<DataType,LengthType,1> result( outSize );

            for( i = 0; i < size[0]; i++ )
            {
                result(i) = ( *this )(i,i);
            }

            return result;
        }
        else
        {
            THROW_EXCEPTION( className, OPERATION_NOT_SUPPORTED );
        }
    }


    /**
    *  Transposes the dimensions of \c this 2D \c Matrix.
    *  \version 1.1.1a
    *  \date    31 January 2011
    *  \returns A \c Matrix class containing the result of the operation.
    *  \throws  OPERATION_NOT_SUPPORTED when \c this matrix has not 2 dimensions.
    */
    template <class DataType, typename LengthType, unsigned char dimensions>
            Matrix<DataType,LengthType,dimensions> Matrix<DataType,LengthType,dimensions>::transpose()
    {
        if( dimensions == 2 )
        {
            LengthType outSize[dimensions];
            outSize[0] = size[1];
            outSize[1] = size[0];
            Matrix<DataType,LengthType,dimensions> result( outSize );

            for( i = 0, k = 0; i < size[0]; i++ )
            {
                for( j = 0; j < size[1]; j++, k++ )
                {
                    result(j,i) = ( *this )(k);
                }
            }

            return result;
        }
        else
        {
            THROW_EXCEPTION( className, OPERATION_NOT_SUPPORTED );
        }
    }


    /**
    *  Calculates the Cholesky decomposition of the matrix if symmetric and positive-definite.
    *  \version 1.2.2a
    *  \date    3 February 2011
    *  \returns The lower triangular \c Matrix, result of calculation.
    *  \throws  OPERATION_NOT_SUPPORTED when \c this matrix has not 2 dimensions.
    *  \throws  MATRIX_NOT_SQUARE when \c this is not a square matrix.
    *  \throws  MATRIX_NOT_SYMMETRIC when \c this is not a symmetric matrix.
    *  \throws  MATRIX_NOT_POSDEFINITE when \c this is not a positive-definite matrix.
    */
    template <class DataType, typename LengthType, unsigned char dimensions>
            Matrix<DataType,LengthType,dimensions> Matrix<DataType,LengthType,dimensions>::cholesky()
    {
        if( dimensions == 2 )
        {
#ifdef DEBUG
            // Check symmetric matrix
            if( !isSymmetric() )
            {
                THROW_EXCEPTION( className, QObject::tr( "Matrix is not symmetric" ) );
            }
#endif

            LengthType outSize[dimensions];
            outSize[0] = size[0];
            outSize[1] = size[1];
            Matrix<DataType,LengthType,dimensions> result( outSize );

            for( j = 0; j < size[0]; j++ )
            {
                result(j,j) = ( *this )(j,j);

                for( i = 1; i <= j; i++ )
                {
                    k = i - 1;
                    result(j,j) -= pow( result(j,k), 2 );
                }

#ifdef DEBUG
                if( result(j,j) <= 0 )
                {
                    THROW_EXCEPTION( className, QObject::tr( "Matrix is not positive-definite" ) );
                }
#endif

                result(j,j) = sqrt( result(j,j) );

                for( i = j + 1; i < size[0]; i++ )
                {
                    result(i,j) = ( *this )(i,j);

                    for( LengthType l = 1; l <= j; l++ )
                    {
                        k = l - 1;
                        result(i,j) -= result(i,k) * result(j,k);
                    }

                    result(i,j) /= result(j,j);
                }
            }

            return result;
        }
        else
        {
            THROW_EXCEPTION( className, OPERATION_NOT_SUPPORTED );
        }
    }


    /**
    *  Inverts the square matrix, if not singular.
    *  \version 1.1.0a
    *  \date    9 August 2010
    *  \returns \c true if successful and \c false otherwise.
       \note Works only for float/double?
       \todo This function needs revision.
    */
    template <class DataType, typename LengthType, unsigned char dimensions> bool Matrix<DataType,LengthType,dimensions>::inv()
    {
        if (this->rowscount != this->columnscount)
            return false;


        if ((this->rowscount == 0) | (this->rowscount == 1))
            return false;  // sanity check

        for (unsigned int i=1; i < this->rowscount; i++) data[i] /= data[0]; // normalize row 0

        for (unsigned int i=1; i < this->rowscount; i++)  {

            for (unsigned int j=i; j < this->rowscount; j++)  { // do a column of L

                DataType sum = 0.0;
                for (unsigned int k = 0; k < i; k++)
                    sum += data[j*this->rowscount+k] * data[k*this->rowscount+i];

                data[j*this->rowscount+i] -= sum;
            }

            if (i == this->rowscount-1) continue;

            for (unsigned int j=i+1; j < this->rowscount; j++)  {  // do a row of U
                DataType sum = 0.0;
                for (unsigned int k = 0; k < i; k++)
                    sum += data[i*this->rowscount+k]*data[k*this->rowscount+j];

                data[i*this->rowscount+j] = (data[i*this->rowscount+j]-sum) / data[i*this->rowscount+i];
            }
        }

        for ( unsigned int i = 0; i < this->rowscount; i++ )  // invert L
            for (unsigned int j = i; j < this->rowscount; j++ )  {
            DataType x = 1.0;
            if ( i != j ) {
                x = 0.0;
                for (unsigned int k = i; k < j; k++ )
                    x -= data[j*this->rowscount+k]*data[k*this->rowscount+i];
            }
            data[j*this->rowscount+i] = x / data[j*this->rowscount+j];
        }
        for (unsigned int i = 0; i < this->rowscount; i++ )   // invert U
            for (unsigned int j = i; j < this->rowscount; j++ )  {
            if ( i == j ) continue;
            DataType sum = 0.0;
            for (unsigned int k = i; k < j; k++ )
                sum += data[k*this->rowscount+j]*( (i==k) ? 1.0 : data[i*this->rowscount+k] );
            data[i*this->rowscount+j] = -sum;
        }
        for (unsigned int i = 0; i < this->rowscount; i++ )   // final inversion
            for (unsigned int j = 0; j < this->rowscount; j++ )  {
            DataType sum = 0.0;
            for ( unsigned int k = ((i>j)?i:j); k < this->rowscount; k++ )
                sum += ((j==k)?1.0:data[j*this->rowscount+k])*data[k*this->rowscount+i];
            data[j*this->rowscount+i] = sum;
        }

        return true;
    }


    //***Debug only***

#ifdef DEBUG
    /**
    *  Prints the matrix in the console.
    *  \version 1.0.1a
    *  \date    1 February 2011
    *  \param [in] format The format to be used for each elements of \c this matrix.
    */
    template <class DataType, typename LengthType, unsigned char dimensions>
            void Matrix<DataType,LengthType,dimensions>::print( char format[] )
    {
        printf( "\n" );

        for( i = 0; i < this->getSize( 0 ); i++ )
        {
            printf( " [ " );

            for( j = 0; j < this->getSize( 1 ); j++ )
            {
                printf( ( string( format ) + string( " " ) ).c_str(), ( *this )(i,j) );
            }

            printf( "]\n" );
        }
    }
#endif

}


#define Matrix(name, DataType, LengthType, dimensions) \
    Matrix<DataType,LengthType,dimensions> name((const LengthType [dimensions])     //!< Macro for \c Matrix initialization
#define MatrixPtr(name, DataType, LengthType, dimensions) \
    Matrix<DataType,LengthType,dimensions> *name = new Matrix<DataType,LengthType,dimensions> ((const LengthType [dimensions]) \
                                                                                    //!< Macro for \c Matrix pointer initialization
#define End )                                                                       //!< Macro for closing \c Matrix initialization
